#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     FL,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     BL,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     FR,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     BR,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     Lift,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     Arm,           tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    WristLift,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

#define SERVO_START_POS 0
#define SERVOS_MAX_VALUE 250
#define SERVOS_MIN_VALUE 0
#define SERVOS_WAIT_TIME_10MSEC 1

int wristPos = 0;

int threshold = 20;

void Forward (int power, int duration)						// Positive power means wheels going forward
{
	motor[FL] = power +10;
	motor[FR] = power;
	motor[BR] = power;
	motor[BL] = power;
	wait1Msec(duration);
}

void Backward (int power, int duration)
{
	motor[FL] = -1*power;
	motor[FR] = -1*power +10;
	motor[BR] = -1*power;
	motor[BL] = -1*power;
	wait1Msec(duration);
}

void turnL (int power, int duration)
{
	motor[FL] = -1*power;
	motor[FR] = power;
	motor[BR] = power;
	motor[BL] = -1*power;
	wait1Msec(duration);
}

void turnR (int power, int duration)
{
	motor[FL] = power;
	motor[FR] = -1*power;
	motor[BL] = power;
	motor[BR] = -1*power;
	wait1Msec(duration);
}

void driftL (int power, int duration)
{
	motor[FL] = -1*power;
	motor[FR] = power;
	motor[BL] = power;
	motor[BR] = -1*power;
	wait1Msec(duration);
}

void driftR (int power, int duration)
{
	motor[FL] = power;
	motor[FR] = -1*power;
	motor[BL] = -1*power;
	motor[BR] = power;
	wait1Msec(duration);
}

void NE (int power, int duration)
{
	motor[FL] = power;
	motor[BR] = power;
	motor[FR] = 0;
	motor[BL] = 0;
	wait1Msec(duration);
}

void SE (int power, int duration)
{
	motor[FL] = 0;
	motor[BR] = 0;
	motor[FR] = -1*power;
	motor[BL] = -1*power;
	wait1Msec(duration);
}

void SW (int power, int duration)
{
	motor[FL] = -1*power;
	motor[BR] = -1*power;
	motor[FR] = 0;
	motor[BL] = 0;
	wait1Msec(duration);
}

void NW (int power, int duration)
{
	motor[FL] = 0;
	motor[BR] = 0;
	motor[FR] = power;
	motor[BL] = power;
	wait1Msec(duration);
}

void initializeRobot ()
{
	motor[FL] = 0;
	motor[BR] = 0;
	motor[FR] = 0;
	motor[BL] = 0;
	wait1Msec(100);
}

task main()
{
	initializeRobot();
	while (true)


	{
		getJoystickSettings(joystick);

		if(joy1Btn(3))
		{
			driftR(45,25);
		}

		else if (joy1Btn(1))
		{
			driftL(45,25);
		}

		else if(joystick.joy1_TopHat == 0)
		{
			Forward(55,25);	                                                                		 // FORWARD
		}

		else if(joystick.joy1_TopHat == 2)
		{
			turnR(20,25);				 // TURN RIGHT
		}

		else if(joystick.joy1_TopHat == 4)
		{
			Backward(55,25);		 // BACKWARD
		}

		else if(joystick.joy1_TopHat == 6)
		{
			turnL(20,25);				  // TURN LEFT
		}

		else if(joy1Btn(5))		 // Diagonal Southwest
		{
			SW(55,25);
		}

		else if(joy1Btn(6))  	 // Diagonal Southeast
		{
			SE(55,25);
		}

		else if(joy1Btn(8))    // Diagonal Northeast
		{
			NE(55,25);
		}

		else if(joy1Btn(7))    // Diagonal Northwest
		{
			NW(55,25);
		}

		else
		{
			motor[FL] = 0;
			motor[FR] = 0;
			motor[BL] = 0;
			motor[BR] = 0;
		}

		//This is where ARM PROGRAM Begins

		//Lift

		if(abs(joystick.joy2_y1) > threshold)
		{
			if(joystick.joy2_y1 > 0)
			{
				motor[Lift] = 65;
			}
			else if(joystick.joy2_y1 < 0)
			{
				motor[Lift] = -1;
			}
			else if (joy2Btn(6))
			{
				motor[Lift] = 10; //
			}
		}
		else
		{
			motor[Lift] = 0;
		}

		//Arm

		if(abs(joystick.joy2_y2) > threshold)
		{
			if(joystick.joy2_y2 < 0)
			{
				motor[Arm] = 30;
			}
			else if(joystick.joy2_y2 > 0)
			{
				motor[Arm] = -30;
			}
		}
		else
		{
			motor[Arm] = 0;
		}

		{
			//WRIST

		if(joy2Btn(5))
			{
				if (wristPos < SERVOS_MAX_VALUE)
				{
					wristPos = wristPos+3;
				}
			}
			else if(joy2Btn(7))
			{
				if (wristPos > SERVOS_MIN_VALUE)
				{
					wristPos = wristPos-3;
				}
			}
			servoTarget[WristLift] = wristPos;
			wait10Msec(1);
		}
	}
}
